#include <ros/ros.h> 
#include <serial/serial.h>  
#include <std_msgs/String.h> 
#include <std_msgs/Empty.h> 
#include "arm_moveit_kinematics/arm_data.h"

static int joint_direction[6] = {1,1,-1,1,1,1};
static serial::Serial* serial_ptr = nullptr;
static float joint_position_buffer[6];
static float _buffer[6];
static uint8_t serial_buff[100] ;

void Joint_Callback(const arm_moveit_kinematics::arm_dataConstPtr &msg);

int main(int argc, char* argv[])
{
        ros::init(argc, argv, "serial_node");
        ros::NodeHandle serial_node;
        
        serial::Serial ser;
        
        ros::Subscriber serial_sub = serial_node.subscribe<arm_moveit_kinematics::arm_data>("joint_serial_data", 100, &Joint_Callback); 
        try 
        { 
                ser.setPort("/dev/ttyUSB0"); 
                ser.setBaudrate(115200); 
                //
                serial::Timeout to = serial::Timeout::simpleTimeout(100); 
                ser.setTimeout(to); 
                ser.open(); 
        } 
        catch (serial::IOException& e) 
        { 
                ROS_ERROR_STREAM("Unable to open port "); 
                return -1; 
        } 

        if(ser.isOpen()) 
                ROS_INFO_STREAM("Serial Port initialized"); 
        else 
                ROS_INFO_STREAM("Serial Port not initialized"); 
                // return -1; 
        serial_ptr = &ser;
        ros::spin();
        return 0;
}

void Joint_Callback(const arm_moveit_kinematics::arm_dataConstPtr &msg)
{
        joint_position_buffer[0] = msg->q0 * joint_direction[0];
        joint_position_buffer[1]=  msg->q1 * joint_direction[1];
        joint_position_buffer[2] = msg->q2 * joint_direction[2];
        joint_position_buffer[3] = msg->q3 * joint_direction[3];
        joint_position_buffer[4] = msg->q4 * joint_direction[4];
        joint_position_buffer[5] = msg->q5 * joint_direction[5];
        
        for(int i = 0; i<6; i++)
        {
                ROS_INFO("%.2f    num: %d",  joint_position_buffer[i], i);
        }

        for (size_t i = 0; i < 6; i++)
                memcpy(&serial_buff[i*4], &joint_position_buffer[i], 4);
        if(serial_ptr)
                serial_ptr->write(serial_buff, 6*4);          
}